#include "h.h"
#define SR 1
#define SK 0.01

void filter1(float *epoch,int m,float *bn,float *an,float *out){
    int j;
    out[0]=epoch[0]*bn[0];
    out[1]=epoch[1]*bn[0]+epoch[0]*bn[1]-out[0]*an[1];
    out[2]=epoch[2]*bn[0]+epoch[1]*bn[1]+epoch[0]*bn[2]-out[1]*an[1]-out[0]*an[2];
    
    for(j=3;j<m;j++){
        out[j]=epoch[j]*bn[0]+epoch[j-1]*bn[1]+epoch[j-2]*bn[2]+epoch[j-3]*bn[3]-out[j-1]*an[1]-out[j-2]*an[2]-out[j-3]*an[3];
    }
}

void LP4hzfilter(float *epoch,float *out,int m){
	float bn[]={ 0.00016237,   0.00048711,    0.00048711,   0.00016237};
	float an[]={1.0000,   -2.7737,    2.5724,   -0.7973};
	filter1(epoch,SR*m,bn,an,out);
}

void HP1hzfilter(float *epoch,float *out,int m){
	float bn[]={ 0.9820,   -2.9461,    2.9461,   -0.9820};
	float an[]={1.0000,   -2.9638,    2.9282,   -0.9644};
	filter1(epoch,SR*m,bn,an,out);
}

void LP8hzfilter(float *epoch,float *out,int m){
	float bn[]={ 0.001171,   0.003512,    0.003512,   0.001171};
	float an[]={1.0000,   -2.5482,    2.1928,   -0.6353};
	filter1(epoch,SR*m,bn,an,out);
}

void HP4hzfilter(float *epoch,float *out,int m){
	float bn[]={ 0.9158,   -2.7475,    2.7475,   -0.9158};
	float an[]={1.0000,   -2.8243,    2.6636,   -0.8387};
	filter1(epoch,SR*m,bn,an,out);
}

void LP16hzfilter(float *epoch,float *out,int m){
	float bn[]={0.2073,    0.6219,    0.6219,    0.2073};
	float an[]={1.0000,    0.2788,    0.3539,    0.0258};

	filter1(epoch,SR*m,bn,an,out);
}

void HP8hzfilter(float *epoch,float *out,int m){
	float bn[]={ 0.8386,   -2.5158,    2.5158,   -0.8386};
	float an[]={1.0000,   -2.6489,    2.3567,   -0.7033};
	filter1(epoch,SR*m,bn,an,out);
}

void LP30hzfilter(float *epoch,float *out,int m){
	float bn[]={0.0390,    0.1170,    0.1170,    0.0390};
	float an[]={1.0000,   -1.3278,    0.8092,   -0.1693};

	filter1(epoch,SR*m,bn,an,out);
	
}

void HP16hzfilter(float *epoch,float *out,int m){
	float bn[]={ 0.7023,   -2.1068,    2.1068,   -0.7023};
	float an[]={1.0000,   -2.3002,    1.8248,   -0.4932};
	filter1(epoch,SR*m,bn,an,out);
}

void HP30hzfilter(float *epoch,float *out,int m){
	float bn[]={ 0.5005,   -1.5016,    1.5016,   -0.5005};
	float an[]={1.0000,   -1.6642,    1.0899,   -0.2500};
	filter1(epoch,SR*m,bn,an,out);
}

void LP50hzfilter(float *epoch,float *out,int m){
	float bn[]={ 0.1289,   0.3866,    0.3866,   0.1289};
	float an[]={1.0000,   -0.2982,    0.3569,   -0.0276};
	filter1(epoch,SR*m,bn,an,out);
}

float energy(float *data2,int m){
	float total;
	int n;
	
	total = 0;
	for (n=0;n<SR*m;n++){
		total = total + (data2[n]*data2[n]);
	}
	
	return total;
}

void fdatacacu (float *fdata,int m,float *curepoch){
	
	
	float Ealpha,Ebeta,Edelta,Etheta,Egama;
	float Palpha,Pbeta,Pdelta,Ptheta,total;
	
	float alpha[SR*m];
	float beta[SR*m];
	float delta[SR*m];
	float theta[SR*m];
	float gama[SR*m];
	float middle[SR*m];
	
	LP4hzfilter(curepoch,middle,m);
	HP1hzfilter(middle,delta,m);
		
	LP8hzfilter(curepoch,middle,m);
	HP4hzfilter(middle,theta,m);
		
	LP16hzfilter(curepoch,middle,m);
	HP8hzfilter(middle,alpha,m);
		
	LP30hzfilter(curepoch,middle,m);
	HP16hzfilter(middle,beta,m);
	
	LP50hzfilter(curepoch,middle,m);
	HP30hzfilter(middle,gama,m);
	
	
	Ealpha=energy(alpha,m);
	Ebeta=energy(beta,m);
	Edelta=energy(delta,m);
	Etheta=energy(theta,m);
	Egama=energy(gama,m);
	
	total = Ealpha+Ebeta+Edelta+Etheta;
	Palpha = Ealpha*100.0/total;
	Pbeta = Ebeta*100.0/total;
	Pdelta = Edelta/total;
	Ptheta = Etheta/total;
	
	fdata[0] = Palpha;
	fdata[1] = Pbeta;
	fdata[2] = Pdelta;
	fdata[3] = Ptheta;
    
    fdata[4] = sqrt(Ealpha)*SK;
    fdata[5] = sqrt(Ebeta)*SK;
    fdata[6] = sqrt(Edelta)*SK;
    fdata[7] = sqrt(Etheta)*SK;
    fdata[8] = sqrt(Egama)*SK;
    
}
